#include "fbm320.h"
#include "iic.h"


unsigned int C0, C1, C2, C3, C6, C8, C9, C10, C11, C12;
unsigned long C4, C5, C7;

float real_temperature_Drgree = 0.0; //Unit Drgree
long real_pressure_Pa = 0;  // Unit Pa
long altitude_mm = 0;//altitude value is in unit mm

long raw_temperature = 0;
long raw_pressure = 0;


void fbm320_write_one_byte(unsigned char reg, unsigned char data) 
{
  iic_start();
  if(iic_write_byte(FBM320_ADDR) == 0) {
    iic_write_byte(reg);
    delay_us(30);
    iic_write_byte(data);
    iic_stop();
  }
}

void fbm320_read_bytes(unsigned char reg, int len ,unsigned char *data) 
{
  iic_start();
  if(iic_write_byte(FBM320_ADDR) == 0) {
    iic_write_byte(reg);
    delay_us(30);
    iic_start();
    iic_write_byte(FBM320_ADDR | 0x01);
    unsigned char i = 0;
    do {
      *(data+i) = iic_read_byte(len-i > 1 ?  0 : 1);
      i++;
    }while(i<len);
    iic_stop();
  }
}

void fbm320_read_otp(void)
{
  unsigned char tmp[20] = {0};
  unsigned int R[10] = {0};
  fbm320_read_bytes(REG_FBM320_CALIBRATION_START0, 18, tmp);
  fbm320_read_bytes(REG_FBM320_CALIBRATION_START1, 1,  tmp+18);
  fbm320_read_bytes(REG_FBM320_CALIBRATION_START2, 1,  tmp+19);
  R[0] = (tmp[0] << 8 | tmp[1]);
  R[1] = (tmp[2] << 8 | tmp[3]);
  R[2] = (tmp[4] << 8 | tmp[5]);
  R[3] = (tmp[6] << 8 | tmp[7]);
  R[4] = (tmp[8] << 8 | tmp[9]);
  R[5] = (tmp[10] << 8 | tmp[11]);
  R[6] = (tmp[12] << 8 | tmp[13]);
  R[7] = (tmp[14] << 8 | tmp[15]);
  R[8] = (tmp[16] << 8 | tmp[17]);
  R[9] = (tmp[18] << 8 | tmp[19]);

  C0 = R[0] >> 4;
  C1 = ((R[1] & 0xFF00) >> 5) | (R[2] & 7);
  C2 = ((R[1] & 0xFF) << 1) | (R[4] & 1);
  C3 = R[2] >> 3;
  C4 = ((unsigned long)R[3] << 2) | (R[0] & 3);
  C5 = R[4] >> 1;
  C6 = R[5] >> 3;
  C7 = ((unsigned long)R[6] << 3) | (R[5] & 7);
  C8 = R[7] >> 3;
  C9 = R[8] >> 2;
  C10 = ((R[9] & 0xFF00) >> 6) | (R[8] & 3);
  C11 = R[9] & 0xFF;
  C12 = ((R[0] & 0x0C) << 1) | (R[7] & 7);
}

unsigned char ver = 0;

void fbm320_calculation(long raw_tempe, 
                        float* real_tempe,
                        long raw_press, 
                        long* real_press)
{
  long X01, X02, X03, X11, X12, X13, X21, X22, X23, X24, X25, X26, X31, X32;
  long PP1, PP2, PP3, PP4, CF;
  long DT, DT2;
  long UT = raw_tempe;
  long UP = raw_press;
  
  
    /* calculation for real temperature value*/
    DT = ((UT - 8388608) >> 4) + (C0 << 4);
    X01 = (C1 + 4459L) * DT >> 1;
    X02 = ((((C2 - 256L) * DT) >> 14) * DT) >> 4;
    X03 = (((((C3 * DT) >> 18) * DT) >> 18) * DT);
  if( real_tempe != 0) {
    *real_tempe = 0.01 * (((2500L << 15) - X01 - X02 - X03) >> 15);
  } 
    DT2 = (X01 + X02 + X03) >> 12;
    X11 = ((C5 - 4443L) * DT2);
    X12 = (((C6 * DT2) >> 16) * DT2) >> 2;
    X13 = ((X11 + X12) >> 10) + ((C4 + 120586) << 4);
    X21 = ((C8 + 7180L) * DT2) >> 10;
    X22 = (((C9 * DT2) >> 17) * DT2) >> 12;
    X23 = (X22 >= X21) ? (X22 - X21) : (X21 - X22);
    X24 = (X23 >> 11) * (C7 + 166426);
    X25 = ((X23 & 0x7FF) * (C7 + 166426)) >> 11;
    if ((X22 - X21) < 0)
      X26 = ((0 - X24 - X25) >> 11) + C7 + 166426;
    else
      X26 = ((X24 + X25) >> 11) + C7 + 166426;

    PP1 = ((UP - 8388608) - X13) >> 3;
    PP2 = (X26 >> 11) * PP1;
    PP3 = ((X26 & 0x7FF) * PP1) >> 11;
    PP4 = (PP2 + PP3) >> 10;
    CF = (2097152 + C12 * DT2) >> 3;
    X31 = (((CF * C10) >> 17) * PP4) >> 2;
    X32 = (((((CF * C11) >> 15) * PP4) >> 18) * PP4);
  if(real_press != 0) {
    *real_press = ((X31 + X32) >> 15) + PP4 + 99880;
  }
}

void fbm320_tempe(void)
{
  fbm320_write_one_byte(REG_FBM320_TAKE_MEAS, CMD_TEMPE_MEAS);
}

void fbm320_press(void)
{
  fbm320_write_one_byte(REG_FBM320_TAKE_MEAS, CMD_PRESS_MEAS);
}

void fbm320_measure(char what)
{
  unsigned char buf[3] = {0};
  fbm320_read_bytes(REG_FBM320_READ_MEAS_REG, 3, buf);
  if(what == 0)
    raw_pressure = ((unsigned long)buf[0] << 16) | ((unsigned long)buf[1] << 8) | (unsigned long)buf[2];
  else 
    raw_temperature = ((unsigned long)buf[0] << 16) | ((unsigned long)buf[1] << 8) | (unsigned long)buf[2];;
}

void fbm320_init(void)
{
  unsigned char tmp[2] = {0};
  
  fbm320_write_one_byte(REG_FBM320_SOFTRESET, CMD_FBM320_SOFTRESET);
  delay_us(15000);
  fbm320_read_bytes(REG_PART_ID,1, tmp);
  
  fbm320_read_bytes(REG_FBM320_TAKE_MEAS, 1, tmp);
  fbm320_read_bytes(REG_FBM320_VERSION, 1, tmp+1);
  ver = ((tmp[0] & 0xC0) >> 6) | ((tmp[1] & 0x70) >> 2); //Verion4 = 0x05
  fbm320_read_otp();
 
}


	long long RP, h0, hs0,RH;
        double HP1, HP2;
	long long hs1, dP0;
	long long PP0;
long long pressure_altitude_conversion(long real_pressure)
{

	
	RP = real_pressure * 8;  // unit Pa conver to unit 0.125Pa

	if ( RP >= 824000 ) {
		PP0	=	103	;
		h0	=	-138507	;
		hs0	=	-5252	;
		hs1	=	311	;
	} else if ( RP >= 784000 ) {
		PP0	=	98	;
		h0	=	280531	;
		hs0	=	-5468	;
		hs1	=	338	;
	} else if ( RP >= 744000 ) {
		PP0	=	93	;
		h0	=	717253	;
		hs0	=	-5704	;
		hs1	=	370	;
	} else if ( RP >= 704000 ) {
		PP0	=	88	;
		h0	=	1173421	;
		hs0	=	-5964	;
		hs1	=	407	;
	} else if ( RP >= 664000 ) {
		PP0	=	83	;
		h0	=	1651084	;
		hs0	=	-6252	;
		hs1	=	450	;
	} else if ( RP >= 624000 ) {
		PP0	=	78	;
		h0	=	2152645	;
		hs0	=	-6573	;
		hs1	=	501	;
	} else if ( RP >= 584000 ) {
		PP0	=	73	;
		h0	=	2680954	;
		hs0	=	-6934	;
		hs1	=	560	;
	} else if ( RP >= 544000 ) {
		PP0	=	68	;
		h0	=	3239426	;
		hs0	=	-7342	;
		hs1	=	632	;
	} else if ( RP >= 504000 ) {
		PP0	=	63	;
		h0	=	3832204	;
		hs0	=	-7808	;
		hs1	=	719	;
	} else if ( RP >= 464000 ) {
		PP0	=	58	;
		h0	=	4464387	;
		hs0	=	-8345	;
		hs1	=	826	;
	} else if ( RP >= 424000 ) {
		PP0	=	53	;
		h0	=	5142359	;
		hs0	=	-8972	;
		hs1	=	960	;
	} else if ( RP >= 384000 ) {
		PP0	=	48	;
		h0	=	5874268	;
		hs0	=	-9714	;
		hs1	=	1131	;
	} else if ( RP >= 344000 ) {
		PP0	=	43	;
		h0	=	6670762	;
		hs0	=	-10609	;
		hs1	=	1354	;
	} else if ( RP >= 304000 ) {
		PP0	=	38	;
		h0	=	7546157	;
		hs0	=	-11711	;
		hs1	=	1654	;
	} else if ( RP >= 264000 ) {
		PP0	=	33	;
		h0	=	8520395	;
		hs0	=	-13103	;
		hs1	=	2072	;
	} else {
		PP0	=	28	;
		h0	=	9622536	;
		hs0	=	-14926	;
		hs1	=	2682	;
	}

	dP0	=	RP - PP0 * 8000;
	HP1	=	(( hs0 * dP0 ) * 0.5);
	HP2	=	((( hs1 * dP0 ) / 16384.0 ) * dP0 ) / 16.0;
	RH	=	(long long)(( HP1 + HP2 ) /256.0 ) + h0;

	return RH;
}